#include "TurnRightAndShootState.h"
#include "PDController.h"
#include <time.h>

TurnRightAndShootState::TurnRightAndShootState(void): hasLocation(false)
{
}


TurnRightAndShootState::~TurnRightAndShootState(void)
{
}

Command TurnRightAndShootState::GetCommand(Agent* agent){
	if(!hasLocation){
		if(agent->getTank()->status.compare("alive")==0){
			pickRandomLocation();
		}
		else{
			return Command(agent->getTank()->index, 0, 0, false);
		}
	}
	if(agent->getTank()->status.compare("alive") != 0){
		hasLocation = false;
		return Command(agent->getTank()->index, 0, 0, false);
	}

	Vector2 pos(agent->getTank()->pos[0],agent->getTank()->pos[1]);
	if((waitLoc-pos).magnitude() < 15)
		return Command(agent->getTank()->index, 0, 0, false);
	return PDController::instance()->GetComand(agent->getTank(), waitLoc-pos);
}

void TurnRightAndShootState::pickRandomLocation(){
	srand ( time(NULL) );
	Vector2 pos(-400, 0);

	while(1){
		Vector2 randPos(-rand()%(World::instance()->getSize()/2),
						rand()%World::instance()->getSize() - World::instance()->getSize()/2);
		if((randPos-pos).magnitude() < 300){
			waitLoc = randPos;
			hasLocation = true;
			break;
		}
	}
}